#include "sms_helper.h"
#include <string>
#include <ctime>
#include <chrono>
#include <thread>
#include <fstream>
#include <dirent.h>
#include <unordered_map>
#include <cassert>
#include <cstring>
#include <algorithm>
#include <iostream>
#include <sms_json.hpp>
#include <sms_base64.hpp>
#include <sys/mman.h>
#include <fcntl.h>
#include <unistd.h>
#include <regex>


namespace sms {


RunStat* RunStat::p = new RunStat;
nlohmann::json g_msg_types;
const int g_shm_size = 1024 * 1024 * 120;


std::string _get_pdir(const std::string& path)
{
    size_t last_slash = path.find_last_of("/\\");
    if (last_slash!= std::string::npos)
    {
        return path.substr(0, last_slash);
    }
    return "";
}


nlohmann::json load_msg_types(std::string msg_type_dir)
{
    if (msg_type_dir.empty())
    {
        if (!g_msg_types.empty())  return g_msg_types;
        std::string type_dir1 = "/usr/share/spirems/msgs";
        std::string type_dir2 = "/usr/share/spirems/json_msgs";
        std::vector<std::string> type_dirs;
        type_dirs.push_back(type_dir1);
        type_dirs.push_back(type_dir2);
        for (std::string type_dir : type_dirs)
        {
            std::vector<std::string> file_paths;
            _list_dir(type_dir, file_paths, ".json", "", true);

            for (std::string file_path : file_paths)
            {
                std::ifstream f(file_path);
                nlohmann::json data = nlohmann::json::parse(f);
                if (data.contains("type") && !g_msg_types.contains(data["type"]))
                {
                    g_msg_types[data["type"]] = data;
                }
            }
        }
    }
    else
    {
        std::vector<std::string> file_paths;
        _list_dir(msg_type_dir, file_paths, ".json", "", true);

        for (std::string file_path : file_paths)
        {
            std::ifstream f(file_path);
            nlohmann::json data = nlohmann::json::parse(f);
            if (data.contains("type") && !g_msg_types.contains(data["type"]))
            {
                g_msg_types[data["type"]] = data;
            }
        }
    }
    if (g_msg_types.empty())
    {
        g_msg_types["std_msgs::Null"] = {
            {"type", "std_msgs::Null"},
            {"timestamp", 0.0}
        };
    }
    return g_msg_types;
}

nlohmann::json def_msg(std::string type_need)
{
    if (g_msg_types.empty())
    {
        load_msg_types();
    }
    if (g_msg_types.contains(type_need))
    {
        return g_msg_types[type_need];
    }
    else
    {
        return g_msg_types["std_msgs::Null"];
    }
}


double get_time_sec()
{
    auto now = std::chrono::system_clock::now();
    auto duration = now.time_since_epoch();
    auto microseconds = std::chrono::duration_cast<std::chrono::microseconds>(duration).count();
    return microseconds / 1e6;
}

void msleep(int ms)
{
    std::this_thread::sleep_for(std::chrono::milliseconds(ms));
}


int check_topic_url(const std::string& topic_url)
{
    int error = 0;
    std::regex pattern("^[a-zA-Z0-9_/]*$");

    if (topic_url.length() < 2) {
        error = 201;  // 至少需要 2 个字符
    }
    else if (topic_url[0] != '/') {
        error = 202;  // 需要以 '/' 开头
    }
    else if (!std::regex_match(topic_url, pattern)) {
        error = 203;  // 只能使用 'a-z', '0-9', '_' 或 '/'
    }

    return error;
}

int check_node_name(const std::string& node_name)
{
    int error = 0;
    std::regex pattern("^[a-zA-Z0-9_]*$");

    if (node_name.length() < 2) {
        error = 210;  // 至少需要 2 个字符
    }
    else if (!std::regex_match(node_name, pattern)) {
        error = 211;  // 只能使用 'a-z', '0-9', '_'
    }
    else if (node_name[0] == '_') {
        if (node_name != "_global") {
            error = 216;  // 以 '_' 开头但不是 "_global"
        }
    }

    return error;
}


std::string _hex2string(const std::string &hex_data)
{
    std::stringstream ss;
    for (size_t i=0; i< hex_data.length(); i+=2)
    {
        int hex_value;
        std::stringstream hex_stream(hex_data.substr(i, 2));

        hex_stream >> std::hex >> hex_value;
        ss << static_cast<char>(hex_value);
    }
    return ss.str();
}

std::string _g_msg_header = _hex2string("EAECFBFD");

int _index_msg_header(std::string& data)
{
    int index = -1;
    if (data.size() >= 4)
    {
        index = data.find(_g_msg_header);
    }
    return index;
}


int _decode_msg_header(std::string& data)
{
    int msg_len = 0;
    if (data.size() > 8)
    {
        int* plen = (int*)(data.c_str() + 4);
        msg_len = *plen + 8;
    }
    return msg_len;
}


void _check_msg(std::string& data, std::vector<std::string>& checked_msgs, std::vector<std::string>& parted_msgs, std::vector<int>& parted_lens)
{
    int index = _index_msg_header(data);
    if (index >= 0)
    {
        if (index > 0)
        {
            std::string parted_msg = data.substr(0, index);
            parted_msgs.push_back(parted_msg);
            parted_lens.push_back(0);
        }
        data = data.substr(index, data.length() - index);
        int msg_len = _decode_msg_header(data);
        if (msg_len > 8)
        {
            while (data.size() >= msg_len)
            {
                if (msg_len > 8)
                    checked_msgs.push_back(data.substr(0, msg_len));

                data = data.substr(msg_len, data.length() - msg_len);
                index = _index_msg_header(data);
                if (index >= 0)
                {
                    data = data.substr(index, data.length() - index);
                    msg_len = _decode_msg_header(data);
                    if (msg_len <= 8)
                        break;
                }
                else
                {
                    msg_len = 0;
                    break;
                }
            }
            if (msg_len > 8 && msg_len < 1024 * 1024 * 5)
            {
                parted_msgs.push_back(data);
                parted_lens.push_back(msg_len);
            }
        }
    }
    else if (data.length() > 0)
    {
        parted_msgs.push_back(data);
        parted_lens.push_back(0);
    }
}


std::string encode_msg(nlohmann::json& json_msg)
{
    if (json_msg["timestamp"] == 0.0)
    {
        json_msg["timestamp"] = get_time_sec();
    }
    std::string json_str = json_msg.dump();
    int json_len = json_str.size();

    size_t length = sizeof(int);
    assert(length == 4);
    char *pdata = (char*) &json_len;
    char len_buf[5];
    for (size_t i=0; i<4; i++)
    {
        len_buf[i] = *pdata++;
    }
    len_buf[4] = 0;

    std::string bytes = _g_msg_header + std::string(len_buf, 4) + json_str;
    return bytes;
}


bool decode_msg(std::string& byte_msg, nlohmann::json& json_msg)
{
    bool succ = false;
    if (byte_msg.size() > 8)
    {
        if (byte_msg.substr(0, 4) == _g_msg_header)
        {
            int msg_len = _decode_msg_header(byte_msg);
            if (msg_len == byte_msg.size())
            {
                byte_msg = byte_msg.substr(8, msg_len - 8);
                json_msg = nlohmann::json::parse(byte_msg);
                succ = true;
            }
        }
    }
    return succ;
}


std::vector<std::string> _split(const std::string& srcstr, const std::string& delimeter)
{
    std::vector<std::string> ret(0); //use ret save the spilted reault
    if (srcstr.empty())    //judge the arguments
    {
        return ret;
    }
    std::string::size_type pos_begin = srcstr.find_first_not_of(delimeter); //find first element of srcstr

    std::string::size_type dlm_pos; //the delimeter postion
    std::string temp;               //use third-party temp to save splited element
    while (pos_begin != std::string::npos) //if not a next of end, continue spliting
    {
        dlm_pos = srcstr.find(delimeter, pos_begin); //find the delimeter symbol
        if (dlm_pos != std::string::npos)
        {
            temp = srcstr.substr(pos_begin, dlm_pos - pos_begin);
            pos_begin = dlm_pos + delimeter.length();
        }
        else
        {
            temp = srcstr.substr(pos_begin);
            pos_begin = dlm_pos;
        }
        if (!temp.empty())
            ret.push_back(temp);
    }
    return ret;
}

bool _startswith(const std::string& str, const std::string& start)
{
    size_t srclen = str.size();
    size_t startlen = start.size();
    if (srclen >= startlen)
    {
        std::string temp = str.substr(0, startlen);
        if (temp == start)
            return true;
    }

    return false;
}

bool _endswith(const std::string& str, const std::string& end)
{
    size_t srclen = str.size();
    size_t endlen = end.size();
    if (srclen >= endlen)
    {
        std::string temp = str.substr(srclen - endlen, endlen);
        if (temp == end)
            return true;
    }

    return false;
}



bool _is_file_exist(std::string& fn)
{
    std::ifstream f(fn);
    return f.good();
}

void _list_dir(std::string dir, std::vector<std::string>& files, std::string suffixs, std::string prefix, bool r)
{
    // assert(_endswith(dir, "/") || _endswith(dir, "\\"));
    if (dir.empty())  return;

    DIR *pdir;
    struct dirent *ent;
    std::string childpath;
    std::string absolutepath;
    pdir = opendir(dir.c_str());
    if (!pdir)  return;

    std::vector<std::string> suffixd(0);
    // std::cout << suffixs << std::endl;
    if (!suffixs.empty() && suffixs != "") {
        suffixd = _split(suffixs, "|");
    }

    while ((ent = readdir(pdir)) != NULL) {
        if (ent->d_type & DT_DIR) {
            if (strcmp(ent->d_name, ".") == 0 || strcmp(ent->d_name, "..") == 0) {
                continue;
            }
            if (r) { // If need to traverse subdirectories
                childpath = dir + "/" + ent->d_name;
                // std::cout << childpath << std::endl;
                _list_dir(childpath, files, suffixs, prefix, r);
            }
        }
        else {
            if (suffixd.size() > 0) {
                bool can_push = false, cancan_push = true;
                for (int i = 0; i < (int)suffixd.size(); i++) {
                    if (_endswith(ent->d_name, suffixd[i]))
                        can_push = true;
                }
                if (prefix.size() > 0) {
                    if (!_startswith(ent->d_name, prefix))
                        cancan_push = false;
                }
                if (can_push && cancan_push) {
                    absolutepath = dir + ent->d_name;
                    files.push_back(dir + "/" + ent->d_name); // filepath
                }
            }
            else {
                absolutepath = dir + ent->d_name;
                files.push_back(dir + "/" + ent->d_name); // filepath
            }
        }
    }
    sort(files.begin(), files.end()); //sort names
}



cv::Mat sms2cvimg(const nlohmann::json& msg)
{
    assert(msg["type"] == "sensor_msgs::CompressedImage" || msg["type"] == "memory_msgs::RawImage");
    if (msg["type"] == "memory_msgs::RawImage")
    {
        std::string mem_name = msg["shm_name"];
        int shm_fst = msg["shm_fst"].get<int>();
        if (!RunStat::instance()->shm_subs.contains(mem_name) || 1 == shm_fst)
        {
            init_submem(mem_name);
        }

        int ptr_id = RunStat::instance()->shm_subs[mem_name]["ptr_id"].get<int>();
        char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];

        int h = msg["height"].get<int>();
        int w = msg["width"].get<int>();
        int offset = msg["offset"].get<int>();
        std::string encoding = msg["encoding"];
        int tp = 0;
        if ("8UC1" == encoding)
            tp = CV_8UC1;
        else if ("8UC2" == encoding)
            tp = CV_8UC2;
        else if ("8UC3" == encoding)
            tp = CV_8UC3;
        else if ("8UC4" == encoding)
            tp = CV_8UC4;
        else if ("16UC1" == encoding)
            tp = CV_16UC1;
        else if ("16UC2" == encoding)
            tp = CV_16UC2;
        else if ("16UC3" == encoding)
            tp = CV_16UC3;
        else if ("16UC4" == encoding)
            tp = CV_16UC4;
        else if ("32FC1" == encoding)
            tp = CV_32FC1;
        else if ("32FC2" == encoding)
            tp = CV_32FC2;
        else if ("32FC3" == encoding)
            tp = CV_32FC3;
        else if ("32FC4" == encoding)
            tp = CV_32FC4;
        else
        {
            std::cerr << "Unsupported sms::encoding type" << std::endl;
            std::abort();
        }

        cv::Mat img(h, w, tp, shm_ptr + offset);
        if (RunStat::instance()->shm_ptrs[ptr_id] == MAP_FAILED)
        {
            std::cerr << "Failed to map shared memory" << std::endl;
            std::abort();
        }
        return img.clone();
    }
    else if (msg["type"] == "sensor_msgs::CompressedImage")
    {
        std::string dncoded_base64 = _base64_decode(msg["data"]);

        // QByteArray decoded_base64 = QByteArray::fromBase64(QByteArray::fromStdString(msg["data"].get<std::string>()));
        std::vector<uchar> decoded_vec;
        decoded_vec.assign(dncoded_base64.begin(), dncoded_base64.end());
        cv::Mat cvimg;
        if (msg["format"] == "jpeg" || msg["format"] == "jpg" || msg["format"] == "png" || msg["format"] == "webp")
        {
            cvimg = cv::imdecode(decoded_vec, cv::IMREAD_COLOR);
        }
        return cvimg;
    }
    else
    {
        std::cerr << "Failed to use shared memory in mem2cvimg" << std::endl;
        std::abort();
        return cv::Mat();
    }
}

nlohmann::json cvimg2sms(const cv::Mat& cvimg, const std::string format, const std::string frame_id, const double timestamp)
{
    nlohmann::json img_msg = def_msg("sensor_msgs::CompressedImage");
    if (timestamp > 0)
        img_msg["timestamp"] = timestamp;
    else
        img_msg["timestamp"] = get_time_sec();
    img_msg["format"] = format;
    img_msg["frame_id"] = frame_id;
    img_msg["is_color"] = 1;

    std::vector<uchar> img_encode;
    if (format == "jpg" || format == "jpeg")
    {
        cv::imencode(".jpg", cvimg, img_encode);
    }
    else if (format == "png")
    {
        cv::imencode(".png", cvimg, img_encode);
    }
    else if (format == "webp")
    {
        std::vector<int> quality = {cv::IMWRITE_WEBP_QUALITY, 50};
        cv::imencode(".webp", cvimg, img_encode, quality);
    }
    
    std::string encoded_base64 = std::string(img_encode.begin(), img_encode.end());
    img_msg["data"] = _base64_encode(encoded_base64);
    return img_msg;
}


std::string _base64_encode(const std::string& input)
{
    return base64::to_base64(input);
}

std::string _base64_decode(const std::string& input)
{
    return base64::from_base64(input);
}


std::string get_time_str_msec()
{
    auto now = std::chrono::system_clock::now();
    auto in_time_t = std::chrono::system_clock::to_time_t(now);

    auto milliseconds = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;
    std::ostringstream oss;
    oss << std::put_time(std::localtime(&in_time_t), "%Y-%m-%d %H:%M:%S");
    oss << "." << std::setfill('0') << std::setw(3) << milliseconds.count();
    return oss.str();
}


Rate::Rate(double freq)
{
    this->_freq = freq;
    this->_default_dt = 1.0 / this->_freq;
    this->_sleep_dt = this->_default_dt;
    this->_last_time = 0.0;
    this->_p = 0.01;
}

Rate::~Rate()
{

}

void Rate::sleep()
{
    msleep((int)(this->_sleep_dt * 1000));
    if (this->_last_time)
    {
        double dt = get_time_sec() - this->_last_time;
        double err = dt - this->_default_dt;
        this->_sleep_dt -= (this->_p * err);
        this->_sleep_dt = std::max(0.0, this->_sleep_dt);
    }
    this->_last_time = get_time_sec();
}


void init_pubmem(const std::string mem_name)
{
    int shm_fd = shm_open(mem_name.c_str(), O_CREAT | O_RDWR, 0666);
    if (shm_fd == -1)
    {
        std::cerr << "Failed to create shared memory object" << std::endl;
        std::abort();
    }
    if (ftruncate(shm_fd, g_shm_size) == -1)
    {
        std::cerr << "Failed to set shared memory size" << std::endl;
        std::abort();
    }
    char* shm_ptr = (char*)mmap(0, g_shm_size, PROT_READ | PROT_WRITE, MAP_SHARED, shm_fd, 0);
    if (shm_ptr == MAP_FAILED)
    {
        std::cerr << "Failed to map shared memory" << std::endl;
        std::abort();
    }

    int ptr_id = static_cast<int>(RunStat::instance()->shm_ptrs.size());
    RunStat::instance()->shm_ptrs.push_back(shm_ptr);
    RunStat::instance()->shm_pubs[mem_name] = {
        {"offset", 0},
        {"fd", shm_fd},
        {"ptr_id", ptr_id},
        {"fst", 1}
    };
    // std::cout << RunStat::instance()->shm_pubs.dump() << std::endl;
}


nlohmann::json cvimg2sms_mem(const cv::Mat& cvimg, const std::string mem_name, const std::string frame_id, const double timestamp)
{
    nlohmann::json img_msg = def_msg("memory_msgs::RawImage");
    if (timestamp > 0)
        img_msg["timestamp"] = timestamp;
    else
        img_msg["timestamp"] = get_time_sec();
    img_msg["frame_id"] = frame_id;
    img_msg["height"] = cvimg.rows;
    img_msg["width"] = cvimg.cols;

    if (cvimg.type() == CV_8UC1)
        img_msg["encoding"] = "8UC1";
    else if (cvimg.type() == CV_8UC2)
        img_msg["encoding"] = "8UC2";
    else if (cvimg.type() == CV_8UC3)
        img_msg["encoding"] = "8UC3";
    else if (cvimg.type() == CV_8UC4)
        img_msg["encoding"] = "8UC4";
    else if (cvimg.type() == CV_16UC1)
        img_msg["encoding"] = "16UC1";
    else if (cvimg.type() == CV_16UC2)
        img_msg["encoding"] = "16UC2";
    else if (cvimg.type() == CV_16UC3)
        img_msg["encoding"] = "16UC3";
    else if (cvimg.type() == CV_16UC4)
        img_msg["encoding"] = "16UC4";
    else if (cvimg.type() == CV_32FC1)
        img_msg["encoding"] = "32FC1";
    else if (cvimg.type() == CV_32FC2)
        img_msg["encoding"] = "32FC2";
    else if (cvimg.type() == CV_32FC3)
        img_msg["encoding"] = "32FC3";
    else if (cvimg.type() == CV_32FC4)
        img_msg["encoding"] = "32FC4";
    else
    {
        std::cerr << "Unsupported cv::Mat type" << std::endl;
        std::abort();
    }

    img_msg["shm_name"] = mem_name;

    if (!RunStat::instance()->shm_pubs.contains(mem_name))
    {
        init_pubmem(mem_name);
    }
    int ptr_id = RunStat::instance()->shm_pubs[mem_name]["ptr_id"].get<int>();
    char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];
    int shm_offset = RunStat::instance()->shm_pubs[mem_name]["offset"].get<int>();
    int shm_fst = RunStat::instance()->shm_pubs[mem_name]["fst"].get<int>();
    if (1 == shm_fst)
    {
        img_msg["shm_fst"] = 1;
        RunStat::instance()->shm_pubs[mem_name]["fst"] = 0;
    }
    else
    {
        img_msg["shm_fst"] = 0;
    }

    size_t img_siz = cvimg.total() * cvimg.elemSize();
    if (shm_offset + img_siz > g_shm_size)
    {
        shm_offset = 0;
    }
    std::memcpy(shm_ptr + shm_offset, cvimg.data, img_siz);
    img_msg["offset"] = shm_offset;
    RunStat::instance()->shm_pubs[mem_name]["offset"] = static_cast<int>(shm_offset + img_siz);
    return img_msg;
}


nlohmann::json pcl2sms_mem(const cv::Mat& pcl, const std::string mem_name, const std::string frame_id, const double timestamp)
{
    nlohmann::json pcl_msg = def_msg("memory_msgs::PointCloud");
    if (timestamp > 0)
        pcl_msg["timestamp"] = timestamp;
    else
        pcl_msg["timestamp"] = get_time_sec();
    pcl_msg["frame_id"] = frame_id;
    pcl_msg["height"] = pcl.rows;
    pcl_msg["width"] = pcl.cols;

    if (pcl.type() != CV_32FC1)
    {
        std::cerr << "Currently, pointcloud only supports the 32FC1 type." << std::endl;
        std::abort();
    }
    pcl_msg["encoding"] = "32FC1";
    pcl_msg["shm_name"] = mem_name;

    if (!RunStat::instance()->shm_pubs.contains(mem_name))
    {
        init_pubmem(mem_name);
    }
    int ptr_id = RunStat::instance()->shm_pubs[mem_name]["ptr_id"].get<int>();
    char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];
    int shm_offset = RunStat::instance()->shm_pubs[mem_name]["offset"].get<int>();
    int shm_fst = RunStat::instance()->shm_pubs[mem_name]["fst"].get<int>();
    if (1 == shm_fst)
    {
        pcl_msg["shm_fst"] = 1;
        RunStat::instance()->shm_pubs[mem_name]["fst"] = 0;
    }
    else
    {
        pcl_msg["shm_fst"] = 0;
    }

    size_t img_siz = pcl.total() * pcl.elemSize();
    if (shm_offset + img_siz > g_shm_size)
    {
        shm_offset = 0;
    }
    std::memcpy(shm_ptr + shm_offset, pcl.data, img_siz);
    pcl_msg["offset"] = shm_offset;
    RunStat::instance()->shm_pubs[mem_name]["offset"] = static_cast<int>(shm_offset + img_siz);
    return pcl_msg;
}

cv::Mat sms2pcl(const nlohmann::json& msg)
{
    assert(msg["type"] == "memory_msgs::PointCloud");

    std::string mem_name = msg["shm_name"];
    int shm_fst = msg["shm_fst"].get<int>();
    if (!RunStat::instance()->shm_subs.contains(mem_name) || 1 == shm_fst)
    {
        init_submem(mem_name);
    }

    int ptr_id = RunStat::instance()->shm_subs[mem_name]["ptr_id"].get<int>();
    char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];

    int h = msg["height"].get<int>();
    int w = msg["width"].get<int>();
    int offset = msg["offset"].get<int>();
    std::string encoding = msg["encoding"];
    int tp = CV_32FC1;
    if ("32FC1" != encoding)
    {
        std::cerr << "Currently, pointcloud only supports the 32FC1 type." << std::endl;
        std::abort();
    }

    cv::Mat img(h, w, tp, shm_ptr + offset);
    if (RunStat::instance()->shm_ptrs[ptr_id] == MAP_FAILED)
    {
        std::cerr << "Failed to map shared memory" << std::endl;
        std::abort();
    }
    return img.clone();
}


void init_submem(const std::string mem_name)
{
    int shm_fd = shm_open(mem_name.c_str(), O_RDONLY, 0666);
    if (shm_fd == -1)
    {
        std::cerr << "Failed to create shared memory object" << std::endl;
        std::abort();
    }
    char* shm_ptr = (char*)mmap(0, g_shm_size, PROT_READ, MAP_SHARED, shm_fd, 0);
    if (shm_ptr == MAP_FAILED)
    {
        std::cerr << "Failed to map shared memory" << std::endl;
        std::abort();
    }

    int ptr_id = static_cast<int>(RunStat::instance()->shm_ptrs.size());
    RunStat::instance()->shm_ptrs.push_back(shm_ptr);
    RunStat::instance()->shm_subs[mem_name] = {
        {"fd", shm_fd},
        {"ptr_id", ptr_id}
    };
    // std::cout << RunStat::instance()->shm_subs.dump() << std::endl;
}

/*
cv::Mat mem2cvimg(const nlohmann::json& msg)
{
    if (_startswith(msg["type"], "memory_msgs::"))
    {
        std::string mem_name = msg["shm_name"];
        if (!RunStat::instance()->shm_subs.contains(mem_name))
        {
            init_submem(mem_name);
        }

        int ptr_id = RunStat::instance()->shm_subs[mem_name]["ptr_id"].get<int>();
        char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];

        int h = msg["height"].get<int>();
        int w = msg["width"].get<int>();
        int offset = msg["offset"].get<int>();
        std::string encoding = msg["encoding"];
        int tp = 0;
        if ("8UC3" == encoding)
        {
            tp = CV_8UC3;
        }
        cv::Mat img(h, w, tp, shm_ptr + offset);
        return img.clone();
    }
    else
    {
        std::cerr << "Failed to use shared memory in mem2cvimg" << std::endl;
        std::abort();
        return cv::Mat();
    }
}
*/


void mem_cleanup(int signal)
{
    // std::cout << "[mem_cleanup_subs]" << std::endl;
    for (auto& [key, value] : RunStat::instance()->shm_subs.items())
    {
        int ptr_id = value["ptr_id"].get<int>();
        char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];
        int shm_fd = value["fd"].get<int>();

        munmap(shm_ptr, g_shm_size);
        RunStat::instance()->shm_ptrs[ptr_id] = static_cast<char*>(MAP_FAILED);
        close(shm_fd);
    }
    // std::cout << "[mem_cleanup_pubs]" << std::endl;
    for (auto& [key, value] : RunStat::instance()->shm_pubs.items())
    {
        int ptr_id = value["ptr_id"].get<int>();
        char* shm_ptr = RunStat::instance()->shm_ptrs[ptr_id];
        int shm_offset = value["offset"].get<int>();
        int shm_fd = value["fd"].get<int>();

        munmap(shm_ptr, g_shm_size);
        close(shm_fd);
        shm_unlink(key.c_str());
    }
    std::exit(signal);
}


void mem_init()
{
    std::signal(SIGINT, sms::mem_cleanup);
    std::signal(SIGTERM, sms::mem_cleanup);
}


std::string ec2str(int ec)
{
    return ERROR_CODE_BOOK[std::to_string(ec)];
}


}
